import java.util.Random;
import java.util.Stack;

import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.Delay;

public class Main {
	public static Cell map[][] = new Cell[4][11];
	public static Cell cell;
	static Stack<String> pathS = new Stack<String>();
	static Stack<String> pathReturn = new Stack<String>();
	static final int RIGHT_ANGLE = 93;
	static final int LEFT_ANGLE = 93;
	static String direction = "+y";
	static String visited = " ";
	static String visited2 = " ";
	static int x = 0;
	static int y = 0;
	static int black = 0;
	static int grey = 0;
	static final int Sen_Angle = 92;
	static final int US_CELL_LEFT = 7;
	static final int US_CELL_RIGHT = 19;
	static final int DIST_TH = 15;
	static final float WHEEL_DIAM = 5.6F;
	static final float TRACK_W = 16.25F;
	static UltrasonicSensor us = new UltrasonicSensor(SensorPort.S2);
	static final DifferentialPilot robot = new DifferentialPilot(WHEEL_DIAM,
			TRACK_W, Motor.A, Motor.C, true);
	static LightSensor ls = new LightSensor(SensorPort.S1);

	public static void fillMap() {
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 11; j++) {
				map[i][j] = new Cell();
				if (j == 0) {
					map[i][j].yn = 1;
				} else if (j == 10) {
					map[i][j].y = 1;
				}
				if (i == 0) {
					map[i][j].xn = 1;
				} else if (i == 3) {
					map[i][j].x = 1;
				}

			}
		}
	}

	public static void visitedOperation(int x, int y) {
		if (direction.indexOf("+y") > -1) {
			if (map[x][y].xn == -1) {
				if (SensorRead() > 21) {
					map[x][y].xn = 0;
					if (x - 1 > -1) {
						map[x - 1][y].x = 0;
					}
				} else {
					map[x][y].xn = 1;
					if (x - 1 > -1) {
						map[x - 1][y].x = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].y == -1) {
				if (SensorRead() > 19) {
					map[x][y].y = 0;
					if (y + 1 < 11) {
						map[x][y + 1].yn = 0;
					}
				} else {
					map[x][y].y = 1;
					if (y + 1 < 11) {
						map[x][y + 1].yn = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].x == -1) {
				if (SensorRead() > 21) {
					map[x][y].x = 0;
					if (x + 1 < 4) {
						map[x + 1][y].xn = 0;
					}
				} else {
					map[x][y].x = 1;
					if (x + 1 < 4) {
						map[x + 1][y].xn = 1;
					}
				}
			}
			SensorTurnLeft();
			SensorTurnLeft();
		} else if (direction.indexOf("+x") > -1) {
			if (map[x][y].y == -1) {
				if (SensorRead() > 21) {
					map[x][y].y = 0;
					if (y + 1 < 11) {
						map[x][y + 1].yn = 0;
					}
				} else {
					map[x][y].y = 1;
					if (y + 1 < 11) {
						map[x][y + 1].yn = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].x == -1) {
				if (SensorRead() > 21) {
					map[x][y].x = 0;
					if (x + 1 < 4) {
						map[x + 1][y].xn = 0;
					}
				} else {
					map[x][y].x = 1;
					if (x + 1 < 4) {
						map[x + 1][y].xn = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].yn == -1) {
				if (SensorRead() > 21) {
					map[x][y].yn = 0;
					if (y - 1 > -1) {
						map[x][y - 1].y = 0;
					}
				} else {
					map[x][y].yn = 1;
					if (y - 1 > -1) {
						map[x][y - 1].y = 1;
					}
				}
			}
			SensorTurnLeft();
			SensorTurnLeft();
		} else if (direction.indexOf("-y") > -1) {
			if (map[x][y].x == -1) {
				if (SensorRead() > 21) {
					map[x][y].x = 0;
					if (x + 1 < 4) {
						map[x + 1][y].xn = 0;
					}
				} else {
					map[x][y].x = 1;
					if (x + 1 < 4) {
						map[x + 1][y].xn = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].yn == -1) {
				if (SensorRead() > 21) {
					map[x][y].yn = 0;
					if (y - 1 > -1) {
						map[x][y - 1].y = 0;
					}
				} else {
					map[x][y].yn = 1;
					if (y - 1 > -1) {
						map[x][y - 1].y = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].xn == -1) {
				if (SensorRead() > 21) {
					map[x][y].xn = 0;
					if (x - 1 > -1) {
						map[x - 1][y].x = 0;
					}
				} else {
					map[x][y].xn = 1;
					if (x - 1 > -1) {
						map[x - 1][y].x = 1;
					}
				}
			}
			SensorTurnLeft();
			SensorTurnLeft();
		} else if (direction.indexOf("-x") > -1) {
			if (map[x][y].yn == -1) {
				if (SensorRead() > 19) {
					map[x][y].yn = 0;
					if (y - 1 > -1) {
						map[x][y - 1].y = 0;
					}
				} else {
					map[x][y].yn = 1;
					if (y - 1 > -1) {
						map[x][y - 1].y = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].xn == -1) {
				if (SensorRead() > 21) {
					map[x][y].xn = 0;
					if (x - 1 > -1) {
						map[x - 1][y].x = 0;
					}
				} else {
					map[x][y].xn = 1;
					if (x - 1 > -1) {
						map[x - 1][y].x = 1;
					}
				}
			}
			SensorTurnRight();
			if (map[x][y].y == -1) {
				if (SensorRead() > 21) {
					map[x][y].y = 0;
					if (y + 1 < 11) {
						map[x][y + 1].yn = 0;
					}
				} else {
					map[x][y].y = 1;
					if (y + 1 < 11) {
						map[x][y + 1].yn = 1;
					}
				}
			}
			SensorTurnLeft();
			SensorTurnLeft();
		}
		String edit = x + "-" + y;
		visited += "#" + edit;
	}

	public static void forward(int trv) {
		robot.setTravelSpeed(10);
		robot.travel(trv);
		robot.stop();
	}

	public static void turnRight() {
		turnRight(RIGHT_ANGLE, true);
		if (direction.equals("+y")) {
			direction = "+x";
		} else if (direction.equals("+x")) {
			direction = "-y";
		} else if (direction.equals("-y")) {
			direction = "-x";
		} else if (direction.equals("-x")) {
			direction = "+y";
		}
	}

	public static void turnLeft() {
		turnLeft(LEFT_ANGLE, true);
		if (direction.equals("+y")) {
			direction = "-x";
		} else if (direction.equals("+x")) {
			direction = "+y";
		} else if (direction.equals("-y")) {
			direction = "+x";
		} else if (direction.equals("-x")) {
			direction = "-y";
		}
	}

	public static void right() {
		SensorTurnRight();
		int dist = SensorRead();
		if (dist > 17)
			forward(3);
		else {
			if (dist < 6) {
				while (SensorRead() != 6) {
					forward(-1);
				}
			} else {
				while (SensorRead() != 6) {
					forward(1);
				}
			}
		}
		SensorTurnLeft();

		turnRight();
	}

	public static void left() {
		SensorTurnRight();

		int dist = SensorRead();
		if (dist > 17)
			forward(3);
		else {
			if (dist < 6) {
				while (SensorRead() != 6) {
					forward(-1);
				}
			} else {
				while (SensorRead() != 6) {
					forward(1);
				}
			}
		}

		SensorTurnLeft();

		turnLeft();
	}

	public static void turnRight(double angle, boolean setSpeed) {
		if (setSpeed) {
			robot.setRotateSpeed(90);
		}
		robot.rotate(angle);
		robot.stop();
	}

	public static void turnLeft(double angle, boolean setSpeed) {
		turnRight(angle * -1, setSpeed);
	}

	public static void SensorTurnRight() {
		SensorTurnRight(Sen_Angle);
	}

	public static void SensorTurnRight(int angle) {
		Motor.B.rotate(angle);
		Motor.B.stop(true);
	}

	public static void SensorTurnLeft(int angle) {
		SensorTurnRight(-1 * angle);
	}

	public static void SensorTurnLeft() {
		SensorTurnLeft(Sen_Angle);
	}

	public static int SensorRead() {
		int dist = us.getDistance();
		Delay.msDelay(150);
		return dist;
	}

	public static int LightSensorRead() {
		int lv = ls.getNormalizedLightValue();
		Delay.msDelay(150);
		if (lv < 32 && lv > 27) {
			black++;
		} else if (lv > 32 && lv < 37) {
			Sound.beep();
			grey++;
		}
		return lv;
	}

	public static int atStart() {
		final int turnDegree = 10;
		int dist, minDist = Integer.MAX_VALUE;
		int totalDegree = 0, error = 2;
		while (true) {
			dist = SensorRead();

			if (dist < US_CELL_LEFT + error) { // Too close to the left wall

			} else if (dist > US_CELL_RIGHT + error) { // Too close to the right
														// wall
			} else { // The ultrasonic sensor sees the wall
				break;
			}

			SensorTurnRight(turnDegree);
			totalDegree += turnDegree;
		}

		// Searching the minDist to the wall by turning right
		while (true) {
			dist = SensorRead();
			if (dist < minDist) {
				minDist = dist;
			} else {
				SensorTurnLeft(turnDegree);
				totalDegree -= turnDegree;
				break;
			}
			SensorTurnRight(turnDegree);
			totalDegree += turnDegree;
		}

		minDist = Integer.MAX_VALUE;

		// Searching the minDist to the wall by turning left
		while (true) {
			dist = SensorRead();
			if (dist < minDist) {
				minDist = dist;
			} else if (dist < 255) {
				SensorTurnRight(turnDegree);
				totalDegree += turnDegree;
				break;
			}
			SensorTurnLeft(turnDegree);
			totalDegree -= turnDegree;
		}

		SensorTurnLeft(totalDegree);

		SensorTurnRight();

		dist = SensorRead();
		if (dist < 7)
			forward(-7);
		else if (dist > 17)
			forward(9);
		else if (dist > 12)
			forward(6);
		else if (dist >= 7)
			forward(3);

		SensorTurnLeft();

		turnRight(totalDegree - 90, true);

		// Checking which start cell
		dist = SensorRead();
		SensorTurnRight();
		int dist2 = SensorRead();
		SensorTurnLeft();

		int cellIn = 7, cellDist = 30;

		LCD.drawInt(dist, 0, 0);
		LCD.drawInt(dist2, 1, 1);

		if (dist2 > cellIn + error) {
			if (dist > cellIn + 3 * cellDist - error
					&& dist < cellIn + 3 * cellDist + error) { // pos5
				turnRight();
				forward(cellDist);
				forward(cellDist);
				forward(cellDist);
				turnRight();
			} else if (dist > cellIn + cellDist) { // pos1
				turnRight();
			}
		} else {
			if (dist > cellIn + 3 * cellDist - error
					&& dist < cellIn + 3 * cellDist + error) { // pos2
				turnRight();
				turnRight();
			} else if (dist > cellIn + 2 * cellDist - error
					&& dist < cellIn + 2 * cellDist + error) { // pos3
				turnRight();
				forward(cellDist);
				turnRight();
			} else if (dist > cellIn + cellDist - error
					&& dist < cellIn + cellDist + error) { // pos4
				turnRight();
				forward(cellDist);
				forward(cellDist);
				turnRight();
			} else { // pos6
				turnRight();
				turnRight();
				forward(cellDist);
				forward(cellDist);
				forward(cellDist);
				turnRight();
			}
		}

		return minDist;
	}

	public static int atStartBackup() {
		final int turnDegree = 10;
		int dist, minDist = Integer.MAX_VALUE;
		while (true) {
			dist = SensorRead();

			if (dist < US_CELL_LEFT + 2) { // Too close to the left wall

			} else if (dist > US_CELL_RIGHT + 2) { // Too close to the right
													// wall
			} else { // The ultrasonic sensor sees the wall
				break;
			}

			turnRight(turnDegree, false);
		}

		// Searching the minDist to the wall by turning right
		while (true) {
			dist = SensorRead();
			if (dist < minDist) {
				minDist = dist;
			} else {
				turnLeft(turnDegree, false);
				break;
			}
			turnRight(turnDegree, false);
		}

		minDist = Integer.MAX_VALUE;

		// Searching the minDist to the wall by turning left
		while (true) {
			dist = SensorRead();
			if (dist < minDist) {
				minDist = dist;
			} else if (dist < 255) {
				turnRight(turnDegree, false);
				break;
			}
			turnLeft(turnDegree, false);
		}
		return minDist;
	}

	public static void forwardCalibrated() {
		forwardCalibrated(30);
	}
	
	public static void forwardCalibrated(double d) {
		int dist;

		robot.setTravelSpeed(10);

		int i = 0;
		while (i < 6) {
			dist = SensorRead();
			if(dist<7){
				turnRight(6, true);
			}
			else if (dist < 10) { // Too close to the left wall
				turnRight(3, true);
			} else if (dist > 13 && dist < 20) {
				turnLeft(3, true);
			}
			
			robot.travel(d/6);

			i++;
		}
		robot.travel(1);
	}

	public static void forwardCalibratedRight(double d) {
		int dist;

		robot.setTravelSpeed(10);

		int i = 0;
		while (i < 6) {
			dist = SensorRead();
			if (dist < 10) { // Too close to the right wall
				turnLeft(3, true);
			} else if (dist > 13 && dist < 20) {
				turnRight(3, true);
			}
			robot.travel(d/6);

			i++;
		}
		robot.travel(1);
	}
	public static void fixPosition() {
		int dist = SensorRead();
		if (dist < US_CELL_LEFT + 2) { // Too close to the left wall
			turnRight(4, true);
		} else if (dist > US_CELL_RIGHT + 2) { // Too close to the right wall
			turnLeft(4, true);
		}
	}

	public static void calibrateTurn() {
		SensorTurnRight();
		int dist = SensorRead();
		if (dist > 17)
			forward(5);
		else {
			if (dist < 6) {
				while (SensorRead() != 6) {
					forward(-1);
				}
			} else {
				while (SensorRead() != 6) {
					forward(1);
				}
			}
		}
		Motor.B.rotate(32);
		Motor.B.stop(true);
		dist = SensorRead();
		if (dist <= 6) {
			forward(-4);
		}
		for (int i = 0; i < 4; i++) {
			robot.rotate(18);
			robot.stop();
			if (dist <= 6) {
				forward(-3);
			}
		}
		Motor.B.rotate(-32);
		Motor.B.stop(true);
		Motor.B.rotate(-32);
		Motor.B.stop(true);
		for (int i = 0; i < 4; i++) {
			robot.rotate(18);
			robot.stop();
			if (dist <= 6) {
				forward(-4);
			}
		}
		Motor.B.rotate(32);
		Motor.B.stop(true);
		SensorTurnLeft();
		if (direction.equals("+y")) {
			direction = "-y";
		} else if (direction.equals("+x")) {
			direction = "-x";
		} else if (direction.equals("-y")) {
			direction = "+y";
		} else if (direction.equals("-x")) {
			direction = "+x";
		}

	}

	public static void maze2() {
		boolean b = false;
		while (true) {
			if (visited.indexOf(x + "-" + y) < 0) {
				visited += "#" + x + "-" + y;
				visitedOperation(x, y);
				LightSensorRead();
			}
			if (x == 3 && y == 10) {
				b = true;
				for (int i = 0; i < 4; i++) {
					if (b) {
						for (int j = 0; j < 11; j++) {
							if (visited2.indexOf(i + "-" + j) < 0) {
								if (map[i][j].x == -1 || map[i][j].xn == -1
										|| map[i][j].y == -1
										|| map[i][j].yn == -1) {
									b = false;
								} else {
									visited2 += "#" + x + "-" + y;
									visited += "#" + x + "-" + y;
								}
							}
						}
					}

				}
			}
			if (!b) {
				if (direction.equals("+y")) {
					if (map[x][y].xn == 0
							&& visited2.indexOf((x - 1) + "-" + y) < 0) {
						left();
						forwardCalibrated();
						x--;
						visited2 += "#" + x + "-" + y;
						pathS.push("+x");
					} else if (map[x][y].y == 0
							&& visited2.indexOf(x + "-" + (y + 1)) < 0) {
						forwardCalibrated();
						y++;
						visited2 += "#" + x + "-" + y;
						pathS.push("-y");
					} else if (map[x][y].x == 0
							&& visited2.indexOf((x + 1) + "-" + y) < 0) {
						right();
						forwardCalibrated();
						x++;
						visited2 += "#" + x + "-" + y;
						pathS.push("-x");
					} else {
						String back = pathS.pop();
						while (direction.equals(back)) {
							right();
						}
						forwardCalibrated();
						y--;
					}
				} else if (direction.equals("+x")) {
					if (map[x][y].y == 0
							&& visited2.indexOf(x + "-" + (y + 1)) < 0) {
						left();
						forwardCalibrated();
						y++;
						visited2 += "#" + x + "-" + y;
						pathS.push("-y");
					} else if (map[x][y].x == 0
							&& visited2.indexOf((x + 1) + "-" + y) < 0) {
						forwardCalibrated();
						x++;
						visited2 += "#" + x + "-" + y;
						pathS.push("-x");
					} else if (map[x][y].yn == 0
							&& visited2.indexOf(x + "-" + (y - 1)) < 0) {
						right();
						forwardCalibrated();
						y--;
						visited2 += "#" + x + "-" + y;
						pathS.push("+y");
					} else {
						String back = pathS.pop();
						while (direction.equals(back)) {
							right();
						}
						forwardCalibrated();
						x--;
					}
				} else if (direction.equals("-y")) {
					if (map[x][y].x == 0
							&& visited2.indexOf((x + 1) + "-" + y) < 0) {
						left();
						forwardCalibrated();
						x++;
						visited2 += "#" + x + "-" + y;
						pathS.push("-x");
					} else if (map[x][y].yn == 0
							&& visited2.indexOf(x + "-" + (y - 1)) < 0) {
						forwardCalibrated();
						y--;
						visited2 += "#" + x + "-" + y;
						pathS.push("+y");
					} else if (map[x][y].xn == 0
							&& visited2.indexOf((x - 1) + "-" + y) < 0) {
						right();
						forwardCalibrated();
						x--;
						visited2 += "#" + x + "-" + y;
						pathS.push("+x");
					} else {
						String back = pathS.pop();
						while (direction.equals(back)) {
							right();
						}
						forwardCalibrated();
						y++;
					}
				} else if (direction.equals("-x")) {
					if (map[x][y].yn == 0
							&& visited2.indexOf(x + "-" + (y - 1)) < 0) {
						left();
						forwardCalibrated();
						y--;
						visited2 += "#" + x + "-" + y;
						pathS.push("+y");
					} else if (map[x][y].xn == 0
							&& visited2.indexOf((x - 1) + "-" + y) < 0) {
						forwardCalibrated();
						x--;
						visited2 += "#" + x + "-" + y;
						pathS.push("+x");
					} else if (map[x][y].y == 0
							&& visited2.indexOf(x + "-" + (y + 1)) < 0) {
						right();
						forwardCalibrated();
						y++;
						visited2 += "#" + x + "-" + y;
						pathS.push("-y");
					} else {
						String back = pathS.pop();
						while (direction.equals(back)) {
							right();
						}
						forwardCalibrated();
						x++;
					}
				}
			} else {
				break;
			}
		}

	}

	public static void mazeleft() {
		fillMap();
		boolean b = false;
		while (true) {
			if (visited.indexOf(x + "-" + y) < 0) {
				visitedOperation(x, y);
				LightSensorRead();
			}
			if (x == 3 && y == 10) {
				b = true;
				for (int i = 0; i < 4; i++) {
					if (b) {
						for (int j = 0; j < 11; j++) {
							if (visited.indexOf(i + "-" + j) < 0) {
								if (map[i][j].x == -1 || map[i][j].xn == -1
										|| map[i][j].y == -1
										|| map[i][j].yn == -1) {
									b = false;
								} else
									visited += "#" + x + "-" + y;
							}
						}
					}

				}
				while (!direction.equals("+x")) {
					right();
				}

				if (!b) {
					if (map[x][y].y == 0
							&& visited2.indexOf(x + "-" + (y + 1)) < 0) {
						left();
						forwardCalibrated();
						y++;
						visited2 += "#" + x + "-" + y;
						pathS.push("-y");
					} else if (map[x][y].yn == 0
							&& visited2.indexOf(x + "-" + (y - 1)) < 0) {
						right();
						forwardCalibrated();
						y--;
						visited2 += "#" + x + "-" + y;
						pathS.push("+y");
					} else {
						pathS.push("-x");
						String back = pathS.pop();
						while (direction.equals(back)) {
							right();
						}
						forwardCalibrated();
						x--;
						if (visited2.indexOf(x + "-" + y) < 0) {
							visited2 += "#" + x + "-" + y;
						}
						pathS.push("+x");
					}
				}
				break;
			}
			if (direction.equals("+y")) {
				if (map[x][y].xn == 0) {
					left();
					forwardCalibrated();
					x--;
					pathReturn.push("+x");
				} else if (map[x][y].y == 0) {
					forwardCalibrated();
					y++;
					pathReturn.push("-y");
				} else if (map[x][y].x == 0) {
					right();
					forwardCalibrated();
					x++;
					pathReturn.push("-x");
				} else if (map[x][y].x == 1) {
					calibrateTurn();
					forwardCalibrated();
					y--;
					pathReturn.push("+y");
				}
			} else if (direction.equals("+x")) {
				if (map[x][y].y == 0) {
					left();
					forwardCalibrated();
					y++;
					pathReturn.push("-y");
				} else if (map[x][y].x == 0) {
					forwardCalibrated();
					x++;
					pathReturn.push("-x");
				} else if (map[x][y].yn == 0) {
					right();
					forwardCalibrated();
					y--;
					pathReturn.push("+y");
				} else if (map[x][y].yn == 1) {
					calibrateTurn();
					forwardCalibrated();
					x--;
					pathReturn.push("+x");
				}
			} else if (direction.equals("-y")) {
				if (map[x][y].x == 0) {
					left();
					forwardCalibrated();
					x++;
					pathReturn.push("-x");
				} else if (map[x][y].yn == 0) {
					forwardCalibrated();
					y--;
					pathReturn.push("+y");
				} else if (map[x][y].xn == 0) {
					right();
					forwardCalibrated();
					x--;
					pathReturn.push("+x");
				} else if (map[x][y].xn == 1) {
					calibrateTurn();
					forwardCalibrated();
					y++;
					pathReturn.push("-y");
				}
			} else if (direction.equals("-x")) {
				if (map[x][y].yn == 0) {
					left();
					forwardCalibrated();
					pathReturn.push("+y");
				} else if (map[x][y].xn == 0) {
					forwardCalibrated();
					x--;
					pathReturn.push("+x");
				} else if (map[x][y].y == 0) {
					right();
					forwardCalibrated();
					y++;
					pathReturn.push("-y");
				} else if (map[x][y].y == 1) {
					calibrateTurn();
					forwardCalibrated();
					x++;
					pathReturn.push("-x");
				}
			}

		}
	}

	// Assumes the direction is +x and the head direction is +y.
	public static void climb() {
		SensorTurnRight();
		while (SensorRead() > 7) {
			forward(5); // Enter to the climb cell
		}
		turnRight(); // Turn toward the ramp
		int dist = SensorRead();
		while (dist > 10) {
			SensorTurnLeft();
			forwardCalibrated();
			SensorTurnRight();
			dist = SensorRead();
		}
		LCD.drawInt(dist, 0, 0);
	}

	// Assumes the black represents the first room.
	private static boolean isFirstRoom() {
		int diff = black - grey;
		if (diff == 0) {
			return (new Random()).nextBoolean();
		} else {
			return diff > 0;
		}
	}

	// Assumes the direction is -y and the head direction is -y.
	public static void mazeTop() {
		Cell[][] mapTop = new Cell[4][3];
		int curX = 4, curY = 0;

		turnRight();
		boolean[] doors = new boolean[4];
		boolean isFirst = isFirstRoom();
		if (isFirst)
			LCD.drawString("First", 0, 0);

		int dist;
		for (int i = 0; i < 4; i++) {
			SensorTurnLeft();
			forwardCalibrated(28);
			SensorTurnRight();
			curX--;

			Cell ce = new Cell();

			if (mapTop[3 - i][0] == null) {
				ce.x = 0;
				ce.yn = 1;
				if (i == 3) {
					ce.xn = 1;
				} else {
					ce.xn = 0;
				}
				mapTop[3 - i][0] = ce;
			}

			SensorTurnRight();
			dist = SensorRead();
			if (dist > US_CELL_RIGHT) { // There is an entrance.
				doors[i] = true;
				ce.y = 0;
			} else {
				ce.y = 1;
			}
			SensorTurnLeft();
		}

		// The robot is at the end of the path.
		// The head is turned to front of the robot.

		if (!isFirst) {
			int door = -1;
			for (door = 3; door >= 1; door--) {
				if (doors[door]) {
					break;
				}
			}
			if (door == 3) {
				SensorTurnLeft();
				right();
				SensorTurnRight();
			} else {
				SensorTurnLeft();
				left();
				left();
				SensorTurnRight();
				for (int i = 3; i > door; i--) {
					SensorTurnRight();
					forwardCalibratedRight(28);
					SensorTurnLeft();
					curX--;
				}

				SensorTurnLeft();
				left();
				SensorTurnRight();
			}
		} else {
			SensorTurnLeft();
			left();
			left();
			SensorTurnRight();

			int door = -1;
			for (door = 0; door < 3; door++) {
				if (doors[door]) {
					break;
				}
			}

			for (int i = 3; i > door; i--) {
				SensorTurnRight();
				forwardCalibratedRight(28);
				SensorTurnLeft();
				curX--;
			}
			SensorTurnLeft();
			left();
			SensorTurnRight();
		}

		// It's in front of the room and toward the room.

		// The head is turned to front of the robot.

		while (curX < 4) { // TODO
			SensorTurnLeft();
			forwardCalibrated(28);
			SensorTurnRight();
			if (direction.equals("+x")) {
				curX++;
			} else if (direction.equals("-x")) {
				curX--;
			} else if (direction.equals("+y")) {
				curY++;
			} else {	//-y
				curY--;
			}
			
			if(LightSensorRead()<20){
				Sound.beep();
				LCD.drawString("Princess", 0, 0);
			}
			
			LCD.drawInt(curX,1,1);
			LCD.drawInt(curY,3,3);
			
			Cell curCell;
			if(curX<4 && -1<curX && -1<curY && curY<3){
				curCell= mapTop[curX][curY];
				if(curCell == null){
					curCell = new Cell();
				}
			}else{
				break;
			}
			
			SensorTurnLeft();
			dist = SensorRead();
			SensorTurnRight();
			if (dist > US_CELL_RIGHT) { // No left wall
				turnLeft();
			} else {
				dist = SensorRead();
				if (dist < US_CELL_RIGHT) { // Front wall
					SensorTurnRight();
					dist = SensorRead();
					if (dist > US_CELL_RIGHT) { // No right wall
						SensorTurnLeft();
						SensorTurnLeft();
						right();
						SensorTurnRight();
						SensorTurnRight();
					}
					SensorTurnLeft();
				}
			}
		}

	}

	// assumes robot enters the start of ramp
	// with the direction +x and the head direction +y
	public static void unClimb() {
		SensorTurnRight();
		while (SensorRead() > 7) {
			forward(5); // Enter to the unclimb cell
		}
		turnLeft(); // Turn toward the ramp
		int dist = SensorRead();
		while (dist > 10) {
			SensorTurnLeft();
			forwardCalibrated();
			SensorTurnRight();
			dist = SensorRead();
		}
		LCD.drawInt(dist, 0, 0);

		turnLeft();
		forward(30);
	}

	public static void goBack() {
		while (x != 0 || y != 0) {
			String curDirection = pathReturn.pop();
			while (direction.indexOf(curDirection) > -1) {
				right();
			}
			forwardCalibrated();
		}
	}

	public static void main(String[] args) {
		Sound.beep();
		atStart();
		mazeleft();
		maze2();
		climb();
		mazeTop();
		unClimb();
		goBack();
		// forward(31);
		/*
		 * int i=0; while (true) { int dist = SensorRead(); LCD.drawInt(dist, 0,
		 * i++); Delay.msDelay(1000); } /* boolean notRamp = true; while
		 * (notRamp) { int wallNumber = 0, headTurn = 0; for (int i = 0; i < 3;
		 * i++) { if (SensorRead() > DIST_TH) { break; } else { wallNumber++; if
		 * (i < 2) { headTurn++; SensorTurnRight(); } } } for (int i = 0; i <
		 * headTurn; i++) { SensorTurnLeft(); } switch (wallNumber) { case 0:
		 * turnLeft(); break; case 2: turnRight(); break; default: } forward();
		 * 
		 * } /* while (true) { SensorTurnLeft(); if (SensorRead() > DIST_TH) {
		 * SensorTurnRight(); turnLeft(); forward(); } else { SensorTurnRight();
		 * if (SensorRead() < DIST_TH) { turnRight(); if (LightSensorRead() <
		 * DIST_TH) { turnRight(); } } forward(); }
		 * 
		 * }
		 */
	}
}
